import sensor, image, time,math,pyb
from pyb import UART,LED,Pin, SPI
import json
import ustruct
fbuf = bytearray(320)#建立帧缓冲区，对于每个RGB565像素，帧缓冲区都需2字节
red_threshold   = ( 30,   100,    15,   127,   15,   127)
i=0
num=0

cs  = Pin("P3", Pin.OUT_OD)
rst = Pin("P7", Pin.OUT_PP)
rs  = Pin("P8", Pin.OUT_PP)
bl  = Pin("P6", Pin.OUT_PP)
USE_HORIZONTAL  = True         # 定义横屏
IMAGE_INVER     = True         # 旋转180°

X_MIN_PIXEL = 0
Y_MIN_PIXEL = 0
if USE_HORIZONTAL:
    X_MAX_PIXEL = 128               # 定义屏幕宽度320
    Y_MAX_PIXEL = 128              # 定义屏幕高度240
else:
    X_MAX_PIXEL = 128               # 定义屏幕宽度
    Y_MAX_PIXEL = 128               # 定义屏幕高度

# 常用颜色表 						#可去除
RED     = 0XF800
GREEN   = 0X07E0
BLUE    = 0X001F
BLACK   = 0X0000
YELLOW  = 0XFFE0
WHITE   = 0XFFFF

CYAN    = 0X07FF
BRIGHT_RED = 0XF810
GRAY1   = 0X8410
GRAY2   = 0X4208

sensor.reset()
sensor.set_pixformat(sensor.RGB565)
sensor.set_framesize(sensor.QQVGA)
sensor.skip_frames(time = 2000)
sensor.set_auto_gain(False) # must be turned off for color tracking
sensor.set_auto_whitebal(False) # must be turned off for color tracking
red_threshold_01=(10, 100, 127, 32, -43, 67)
clock = time.clock()

uart = UART(3,115200)   #定义串口3变量
uart.init(115200, bits=8, parity=None, stop=1) # init with given parameters

spi = SPI(2, SPI.MASTER, baudrate=int(10000000000/66*0.06), polarity=0, phase=0,bits=8)

def write_command_byte(c):
    cs.low()
    rs.low()
    spi.send(c)
    cs.high()

# SPI 写数据
def write_data_byte(c):
    cs.low()
    rs.high()
    spi.send(c)
    cs.high()

def write_command(c, *data): #命令数据一起写，先写命令 第二个开始为数据位。如果只写一个，则代表不写数据只写命令。
    write_command_byte(c)
    if data:
        for d in data:
            if d > 255:
                write_data_byte(d >> 8)
                write_data_byte(d&0xFF)
            else:
                write_data_byte(d)



def sending_data(cx,cy,cw,ch):
    global uart;
    data = ustruct.pack("<bbhhhhb",      #格式为俩个字符俩个短整型(2字节)
                   0x2C,                      #帧头1
                   0x12,                      #帧头2
                   int(cx), # up sample by 4   #数据1
                   int(cy), # up sample by 4    #数据2
                   int(cw), # up sample by 4    #数据1
                   int(ch), # up sample by 4    #数据2
                   0x5B)
    uart.write(data);   #必须要传入一个字节数组

K=300#the value should be measured  15*20
K2=0.153#实际大小=K*直径像素     k2=2.3cm/15=0.153

def write_image(img):
    cs.low()
    rs.high()
    if(True): #全发
        for m in img: 					#把一帧图像的对象取出来，放到帧缓存区中
            fbuf=m
            for i in range(0,128):		#每行每行的发送
                spi.send(fbuf[i]>>8)	#先发第N行的第I个数据的高八位
                spi.send(fbuf[i]&0xFF)	#再发低八位
            #print(fbuf)
    else:#只发一行固定的数据
        for i in range(0,128):
            spi.send(fbuf[i])
            spi.send(fbuf[i+1]&0xFF)

    cs.high()

def SetXY(xpos, ypos):
    write_command(0x2A, xpos>>8, xpos&0XFF)
    write_command(0x2B, ypos>>8, ypos&0XFF)
    write_command(0x2C)

def SetRegion(xStar, yStar, xEnd, yEnd):
    write_command(0x2A, xStar>>8, xStar&0XFF, xEnd>>8, xEnd&0XFF)
    write_command(0x2B, yStar>>8, yStar&0XFF, yEnd>>8, yEnd&0XFF)
    write_command(0x2C)

# 在指定位置绘制一个点
def DrawPoint(x, y, Color):
    SetXY(x, y)
    write_data_byte(Color >> 8)
    write_data_byte(Color&0XFF)

def ReadPoint(x, y):
    data = 0
    SetXY(x, y)
    write_data_byte(data)
    return data

def Clear(Color):
    global X_MAX_PIXEL, Y_MAX_PIXEL
    SetRegion(0, 0, X_MAX_PIXEL-1 , Y_MAX_PIXEL-1 )
    #cs.low()
    #rs.high()
    for i in range(0, Y_MAX_PIXEL):
        for m in range(0, X_MAX_PIXEL):
            write_data_byte(Color >> 8)
            write_data_byte(Color&0xFF)

def LCDinit():
    rst.low()
    time.sleep_ms(100)
    rst.high()
    time.sleep_ms(100)

    write_command(0xCB, 0x39, 0x2c, 0x00, 0x34, 0x02)
    write_command(0xCF, 0x00, 0XAA, 0XE0)
    write_command(0xE8, 0x85, 0x11, 0x78)
    write_command(0xEA, 0x00, 0x00)
    write_command(0xED, 0x67, 0x03, 0X12, 0X81)
    write_command(0xF7, 0x20)
    write_command(0xC0, 0x21)       # Power control, VRH[5:0]
    write_command(0xC1, 0x11)       # Power control, SAP[2:0];BT[3:0]
    write_command(0xC5, 0x24, 0x3C) # VCM control, 对比度调节
    write_command(0xC7, 0xB7)       # VCM control2, --

    # Memory Data Access Control
    if USE_HORIZONTAL:      # //C8   //48 68竖屏//28 E8 横屏
        if IMAGE_INVER:
            write_command(0x36, 0xE8)   # 从右到左 e8/68
        else:
            write_command(0x36, 0x28)   # 从左到右 28
    else:
        if IMAGE_INVER:
            write_command(0x36, 0xC8)   # 从下到上刷
        else:
            write_command(0x36, 0x48)   # 从上到下刷

    global X_MAX_PIXEL, Y_MAX_PIXEL
    SetRegion(0, 0, X_MAX_PIXEL - 1, Y_MAX_PIXEL - 1)

    # Interface Pixel Format
    write_command(0x3A, 0x55)

    write_command(0xB1, 0x00, 0x18)
    write_command(0xB6, 0x08, 0x82, 0x27)   # Display Function Control
    write_command(0xF2, 0x00)               # 3Gamma Function Disable
    write_command(0x26, 0x01)               # Gamma curve selected
    write_command(0xB7, 0x06)

    # set gamma
    write_command(0xE0, 0x0F, 0x31, 0x2B, 0x0C, 0x0E, 0x08, 0x4E, 0xF1, 0x37, 0x07, 0x10, 0x03, 0x0E, 0x09, 0x00)
    write_command(0XE1, 0x00, 0x0E, 0x14, 0x03, 0x11, 0x07, 0x31, 0xC1, 0x48, 0x08, 0x0F, 0x0C, 0x31, 0x36, 0x0F)

    write_command(0x11) # sleep_ms Exit
    time.sleep_ms(120)

    # Display On
    write_command(0x29)
    write_command(0x2C)
    bl.high()   # 拉背光

def display(img):
    write_command(0x2C)
    write_image(img)

LCDinit() # Initialize the lcd screen.
Clear(BLUE)
#DrawPoint(50, 50, RED)
display(sensor.snapshot())

while(True):
    pass
    display(sensor.snapshot())
    clock.tick()
    img = sensor.snapshot()
    blobs = img.find_blobs([red_threshold])     #监测红色
    if len(blobs) == 1:
        # Draw a rect around the blob.
        b = blobs[0]
        img.draw_rectangle(b[0:4]) # rect
        img.draw_cross(b[5], b[6]) # cx, cy
        Lm = (b[2]+b[3])/2
        length = K/Lm
        leng=int(length)
        print(length)   #距离
        #print(Lm)  #像素数

        size=K2*Lm
        #print(size)
        h=K2*b[3]
        w=K2*b[2]
        high=int(h)
        wide=int(w)
        print("高：%s cm，宽：%scm"%(high,wide))

    for r in img.find_rects(threshold = 50000):
       img.draw_rectangle(r.rect(), color = (255, 0, 0))
       i=1
       num+=1

    if i==1:
       print("监测到矩形",num)
       FH = bytearray([0x2C,0x12,num,leng,high,wide,0x5B])
       uart.write(FH)




